#include <stdio.h>
#include <wiringPi.h>
#include <softPwm.h>
#include <pthread.h>
#include "motor.h"
#include "ops.h"
#include "angle.h"
#include "ring.h"
#include <signal.h>

int D_x_one;
int D_x[6] = {0};
int D_y[6] = {0};
int pic_null = -1;
int action_start_2func = 0;
int res;
int result[2] = {-1,-1};
int time_number = 0;
int car_calibrate_flag = 0;
int ripe_flag = 0;
int cam_start=0;
int pass_flag = 1;
int ripe_num = 0;
int arm_adjust_flag = 0;
int targ_pos_cam_x = 290;
int targ_pos_cam_y = 160;
int adjust_flag_x = 1;
int adjust_flag_y = 1;
int num_flag = 0;
int num_flag_x = 0;
int col_green_num = 0;
int pos_select;
int angle_corrective=0;

int speed=15;
int target_pos_x=341,target_pos_y=241;//lv:320 263 y++向上 x++向左
int shuzi_shen;
int pid_busy1;
float target_angle=0;
int bobao=0;
pthread_t t1;
pthread_t t3;
pthread_t t4;
pthread_t t5;
pthread_t t6;
pthread_t t_fang;
pthread_t t7;
static void my_handler(int sig){ // can be called asynchronously
	stop();
	exit(0);
}
void L298N_13(float speed,float angle)							//速度调整函数
{
	softPwmWrite(PWM1,speed+angle);
	softPwmWrite(PWM2,0);
	softPwmWrite(PWM3,speed-angle);
	softPwmWrite(PWM4,0);
}

void L298N_tioazheng(float speed,float angle)				//调整时速度调整函数
{
	softPwmWrite(PWM1,speed+angle);
	softPwmWrite(PWM2,speed-angle);
	softPwmWrite(PWM3,speed-angle);
	softPwmWrite(PWM4,speed+angle);
}

void L298N_back(float speed,float angle)					//倒着走速度调整函数
{
	softPwmWrite(PWM1,speed-angle);
	softPwmWrite(PWM2,speed+angle);
	softPwmWrite(PWM3,speed+angle);
	softPwmWrite(PWM4,speed-angle);
}

//~ void *fun1(void *arg)		//前进PID
//~ {
	//~ while(1)
	//~ {
    
		//~ while (pid_flag==1)
		//~ {	pid_busy=1;
			//~ if(angle-target_angle<-0.3)
			//~ {
				//~ move_1(1,15);
				//~ move_3(1,15);
				//~ move_2(1,15);
				//~ move_4(1,15);
				//~ delay(10);
				//~ stop_2();
				//~ stop_4();
			//~ }
			//~ if(angle-target_angle>0.3)
			//~ {
				//~ move_2(0,15);
				//~ move_4(1,15);
				//~ delay(10);
				//~ stop_2();
				//~ stop_4();
			//~ }
			//~ if(x_pos-target_xx>2)
			//~ {
				//~ move_2(1,20);
				//~ move_4(1,20);
				//~ delay(10);
				//~ stop_2();
				//~ stop_4();
			//~ }
			//~ if(x_pos-target_xx<-2)
			//~ {
				//~ move_2(0,20);
				//~ move_4(0,20);
				//~ delay(10);
				//~ stop_2();
				//~ stop_4();
			//~ }
			//~ pid_busy=0;
			//~ delay(20);
		
		  //~ }
		   
		//~ while (pid_flag==2)
		//~ {	
			//~ pid_busy=1;
			//~ if(angle-target_angle<-0.3)
			//~ {
				//~ move_1(0,15);
				//~ move_3(1,15);
				//~ delay(10);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ if(angle-target_angle>0.3)
			//~ {
				//~ move_1(1,15);
				//~ move_3(0,15);
				//~ delay(10);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ if(y_pos-target_yy>2)
			//~ {
				//~ move_1(0,20);
				//~ move_3(0,20);
				//~ delay(10);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ if(y_pos-target_yy<-2)
			//~ {
				//~ move_1(1,20);
				//~ move_3(1,20);
				//~ delay(10);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ pid_busy=0;
			//~ delay(20);
		//~ }
		//~ while(pid_flag==3)
		//~ {
			//~ pid_busy1=1;
			
			//~ if(fabs(x-target_pos_x)>10)
			//~ {
					
					//~ move_x_y_xiang(dir_x,(x-target_pos_x)/3,15,0);
					//~ printf("xx=%d\n",num_x-target_pos_x);
					//~ //printf("num= %d yy=%d\n",x,-(x-target_pos_x)/5);
					//~ delay(500);
					
			//~ }
			//~ else
			//~ {
				//~ if(x-target_pos_x<0)
				//~ left();
				//~ else
				//~ right();
				//~ softPwmWrite(PWM2,20);
				//~ softPwmWrite(PWM4,20);
				//~ delay(20);
				//~ stop();
				//~ delay(500);
				//~ }
			//~ //while(end);
			//~ if(fabs(y-target_pos_y)>10)
			//~ {
					//~ //printf("num= %d yy=%d\n",y,-(y-target_pos_y)/5);
					//~ //if(y-target_pos_y)
					//~ move_x_y_xiang(dir_y,-(y-target_pos_y)/3,15,0);
					
					//~ delay(1000);
			//~ }
			//~ else
			//~ {
				//~ if(y-target_pos_y<0)
				//~ front();
				//~ else
				//~ back();
				//~ softPwmWrite(PWM1,20);
				//~ softPwmWrite(PWM3,20);
				//~ delay(20);
				//~ stop();
				//~ delay(1000);
				
				//~ }
				//~ pid_busy1=0;
			//~ }
			//~ while(pid_flag==4)
			//~ {
				//~ unsigned char s;
				 //~ if(serialDataAvail(fd_1)>0)
				 //~ {
					 //~ s=serialGetchar(fd_1);	
					  //~ serialFlush (fd_1);					
				//~ }
				//~ printf("1=%x,2=%x,3=%x,4=%x\n",(s&0x01)!=0x01,(s&0x02)!=0x02,(s&0x04)!=0x04,((s&0x08))!=0x08);
				
				//~ if(((s&0x01)==0x01)&&((s&0x02)!=0x02)&&((s&0x04)!=0x04)&&((s&0x08)!=0x08))
				//~ {
					//~ move_1();
					//~ move_3();
					//~ softPwmWrite(PWM1,20);
					//~ softPwmWrite(PWM3,20);
					//~ delay(20);
					//~ stop_1();
					//~ stop_3();
					//~ }
				//~ if(((s&0x01)!=0x01)&&((s&0x02)==0x02)&&((s&0x04)!=0x04)&&((s&0x08)!=0x08))
				//~ {
					//~ move_1();
					//~ move_3();
					//~ softPwmWrite(PWM1,20);
					//~ softPwmWrite(PWM3,20);
					//~ delay(10);
					//~ stop_1();
					//~ stop_3();
					//~ }
				//~ if(((s&0x01)!=0x01)&&((s&0x02)!=0x02)&&((s&0x04)==0x04)&&((s&0x08)!=0x08))
				//~ {
					//~ move_1();
					//~ move_3();
					//~ softPwmWrite(PWM1,20);
					//~ softPwmWrite(PWM3,20);
					//~ delay(10);
					//~ stop_1();
					//~ stop_3();
					//~ }
				//~ if(((s&0x01)!=0x01)&&((s&0x02)!=0x02)&&((s&0x04)!=0x04)&&((s&0x08)==0x08))
				//~ {
					//~ move_1();
					//~ move_3();
					//~ softPwmWrite(PWM1,20);
					//~ softPwmWrite(PWM3,20);
					//~ delay(20);
					//~ stop_1();
					//~ stop_3();
					//~ }
					
				//~ delay(10); 
				//~ }
		//~ while(pid_flag==5)
		//~ {
			//~ pid_busy=1;
			//~ if(angle-target_angle<-0.3)
			//~ {
				//~ move_1(0,15);
				//~ move_3(1,15);
				//~ delay(15);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ if(angle-target_angle>0.3)
			//~ {
				//~ move_1(1,15);
				//~ move_3(0,15);
				//~ delay(15);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ pid_busy=0;
			//~ delay(30);
		//~ }
		//~ while(pid_flag==6)
		//~ {
			//~ pid_busy=1;
			//~ if(angle-target_angle<-0.3)
			//~ {
				//~ move_2(1,15);
				//~ move_4(0,15);
				//~ delay(10);
				//~ stop_2();
				//~ stop_4();
			//~ }
			//~ if(angle-target_angle>0.3)
			//~ {
				//~ move_2(0,15);
				//~ move_4(1,15);
				//~ delay(10);
				//~ stop_2();
				//~ stop_4();
			//~ }
			//~ pid_busy=0;
			//~ delay(20);
		//~ }
		//~ while(pid_flag==7)
		//~ {
			//~ pid_busy=1;
			//~ if(angle-target_angle<-0.3)
			//~ {
				//~ //move_2(1,15);
				//~ move_4(0,15);
				//~ delay(15);
				//~ //stop_2();
				//~ stop_4();
			//~ }
			//~ if(angle-target_angle>0.3)
			//~ {
				//~ //move_2(0,15);
				//~ move_4(1,15);
				//~ delay(15);
				//~ //stop_2();
				//~ stop_4();
			//~ }
			//~ pid_busy=0;
			//~ delay(20);
		//~ }
		//~ while(pid_flag==7)
		//~ {
			//~ pid_busy=1;
			//~ if(angle-target_angle<-0.3)
			//~ {
				//~ move_1(0,15);
				//~ move_3(1,15);
				//~ delay(15);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ if(angle-target_angle>0.3)
			//~ {
				//~ move_1(1,15);
				//~ move_3(0,15);
				//~ delay(15);
				//~ stop_1();
				//~ stop_3();
			//~ }
			//~ pid_busy=0;
			//~ delay(17);
		//~ }
	//~ }
//~ }	

void *fun1(void *arg)//pid3
{  
	int i;
	while (1)
	{	
		float kp=4;
		float ki=0;
		float kd=3;
		float this_error=0,last_error,kp_error,ki_error,kd_error,angle1,angle2;
		float sum;
		for(i=1;i>0;i++)
	    { 
			angle=angle+angle_corrective;
			kp_error=angle-target_angle;
			ki_error+=angle-target_angle;
			if(ki_error>=5)ki_error=5;
			if(i%2==1)
			{last_error=angle-target_angle;}
			if(i%2==0)
			{this_error=angle-target_angle;}
			if(i%2==0)
			{kd_error=this_error-last_error;}
			if(i%2==1)
			{kd_error=last_error-this_error;}
			PID_OUT_angle=kp*kp_error+ki*ki_error+kd*kd_error;
			if(PID_OUT_angle>30)
			{PID_OUT_angle=30;}
			if(PID_OUT_angle<-30)
			{PID_OUT_angle=-30;}
			delay(80);
			//~ printf("x_pos=%f\n",x_pos);
				//~ printf("x_pos_tem=%f\n",x_pos_tem);
				//~ printf("y_pos=%f\n",y_pos);
				//~ printf("y_pos_tem=%f\n",y_pos_tem);
				//~ printf("angle=%f\n",angle);
			//~ printf("target_angle=%f\n",target_angle);
		}
    }
}
float PID_OUT_pos;
void *fun6(void *arg)//pid3
{  
	float target_pos=0,now_pos=0;
	int i;
	while (1)
	{	
		float kp=4;
		float ki=0;
		float kd=3;
		float this_error=0,last_error,kp_error,ki_error,kd_error;
		//float sum;
		for(i=1;i>0;i++)
	    { 
			if(pid_flag==1)
			{
				float kp=4;
				float ki=0;
				float kd=3;
				target_pos=target_xx;
				now_pos=x_pos;
			}
			if(pid_flag==2)
			{
				float kp=4;
				float ki=0;
				float kd=3;
				target_pos=target_yy;
				now_pos=y_pos;
			}
			kp_error=now_pos-target_pos;
			ki_error+=now_pos-target_pos;
			if(ki_error>=20)ki_error=20;
			if(i%2==1)
			{last_error=now_pos-target_pos;}
			if(i%2==0)
			{this_error=now_pos-target_pos;}
			if(i%2==0)
			{kd_error=this_error-last_error;}
			if(i%2==1)
			{kd_error=last_error-this_error;}
			PID_OUT_pos=kp*kp_error+ki*ki_error+kd*kd_error;
			if(PID_OUT_pos>30)
			{PID_OUT_pos=30;}
			if(PID_OUT_pos<-30)
			{PID_OUT_pos=-30;}
			delay(40);
			//~ printf("now_pos=%f\n",now_pos);
			//~ printf("pid_flag=%f\n",pid_flag);
			//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
			//~ printf("now_pos=%f\n",now_pos);
			//~ printf("1111111111111111111111111111111111111111");
			//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
			//~ printf("PID_OUT_angle=%f\n",PID_OUT_angle);
			//~ printf("target_xx=%f\n",target_xx);
			//~ printf("target_yy=%f\n",target_yy);
			//~ printf("x_pos=%f\n",x_pos);
			//~ printf("y_pos=%f\n",y_pos);
			//~ printf("angle=%f\n",angle);
			//~ printf("\n");
		}
    }
}

void *fun7(void *arg)
{
	while(1)
	{
	while(cam_adjust==1)
	{
		if(fabs(x-target_pos_x)>10)
			{
					printf("111");
				move_x_y_xiang(dir_x,(x-target_pos_x)/3,15,0);
				//~ printf("xx=%d\n",num_x-target_pos_x);
				//printf("num= %d yy=%d\n",x,-(x-target_pos_x)/5);
				delay(500);
				
			}
			//~ else
			//~ {
				//~ if(x-target_pos_x<0)
				//~ right();
				//~ else
				//~ left();
				//~ softPwmWrite(PWM2,20);
				//~ softPwmWrite(PWM4,20);
				//~ delay(20);
				//~ stop();
				//~ delay(500);
				//~ }
			//~ //while(end);
			//~ if(fabs(y-target_pos_y)>10)
			//~ {
					//~ //printf("num= %d yy=%d\n",y,-(y-target_pos_y)/5);
					//~ //if(y-target_pos_y)
					//~ move_x_y_xiang(dir_y,-(y-target_pos_y)/3,15,0);
					
					//~ delay(1000);
			//~ }
			//~ else
			//~ {
				//~ if(y-target_pos_y<0)
				//~ front();
				//~ else
				//~ back();
				//~ softPwmWrite(PWM1,20);
				//~ softPwmWrite(PWM3,20);
				//~ delay(20);
				//~ stop();
				//~ delay(1000);
				
				//~ }
				//~ pid_busy1=0;
			//~ }
	}
}
}

void my_handler_1()
{
	pthread_create(&t4,NULL,ring_cpp,NULL);
	}
	


/*************************驱动设备初始化**************************
 * 波特率115200
 * 树莓派4B的串口1
 * 发送数据包
 * 
 * 
 * ***/

//~ int fd_1;
//~ void duoji_init()
//~ {
	//~ fd_1=serialOpen("/dev/ttyAMA1",115200);
	//~ if(fd_1<0)
	//~ {
		//~ printf("duoji open fail!\r\n");
	//~ }
	//~ else
	//~ printf("Time:%d,舵机启动成功\n",millis());
//~ }

//~ void set_angle(unsigned char ID,int agl,unsigned int tim)
//~ {
	//~ unsigned char pos1,pos2,interval1,interval2,Usum;
	//~ unsigned char f;
	
	//~ if (agl>=0)
	//~ {
        //~ pos1 = agl%256;
        //~ pos2 = agl/256;
	//~ }
	//~ if (agl<0)
	//~ {
        //~ pos1 = (0-agl)%256;
        //~ pos2 = (0-agl)/256;
        //~ pos1 = 255-pos1;
        //~ pos2 = 255-pos2;
	//~ }
	//~ interval1 = tim%256;
	//~ interval2 = tim/256;
	
	//~ Usum = (0x12+0x4c+0x08+0x07+ID+pos1+pos2+interval1+interval2)%256;
	
	//~ unsigned char byte[12]={0x12,0x4c,0x08,0x07,ID,pos1,pos2,interval1,interval2,0,0,Usum};
	//~ for(f=0;f<12;f++)
	//~ {serialPutchar(fd_1,byte[f]);}
//~ }

/*************************机械臂初始时舵机角度******************************/
//4 up+  3up-
int init_angle[5] = {100,500,850,700,300};
int now_angle[5] = {0};  //记录各舵机的当前角度

/*************************更新版舵机驱动****************************/

void set_angle_pro(unsigned char ID,int agl,unsigned int tim)
{
	set_angle(ID,agl,tim);
	now_angle[ID-1] = agl;
}

/************************机械臂位置初始化（爪子张开）****************************/

void pos_init()
{
	set_angle(1,100,1000);		//+ ->right
	set_angle(2,500,1000);   	//+ ->down
	set_angle(3,850,1000);		//+ ->down
	set_angle(4,700,1000);		//- -> up
	set_angle(5,300,1000);		//
	delay(3000);
	}

/************************机械臂位置初始化（爪子闭合开）****************************/

void pos_init_close()
{
	//set_angle_pro(1,100,2000);		//+ ->right
	set_angle_pro(2,500,2000);   	//+ ->down
	set_angle_pro(3,850,2000);		//+ ->down
	set_angle_pro(4,700,2000);		//- -> up
	//set_angle_pro(5,-100,1000);		//
	delay(3000);
	}

/***********************机械臂正曲抓取位置初始化************************/
int init_arm_angle[5] = {1100,500,850,700,300};

void forward_arm_init()
{
	//set_angle_pro(1,init_arm_angle[0],1000);		//+ ->right
	set_angle_pro(2,init_arm_angle[1],1000);   	//+ ->down
	set_angle_pro(3,init_arm_angle[2],1000);		//+ ->down
	set_angle_pro(4,init_arm_angle[3],1000);		//- -> up
	set_angle_pro(5,init_arm_angle[4],1000);		//
	delay(1000);
}


//正值 -> 机械臂向前  
void arm_around(int delta)
{
	set_angle_pro(3,now_angle[2] - delta, 2000);		//+ ->down
	set_angle_pro(4,now_angle[3] - delta, 2000);		//- -> up
	delay(2000);
}

// 正值 -> 机械臂向后
void adjustment_up_dowm(int param)		
{
	set_angle_pro(2,now_angle[1] + param, 1000);   		//+ ->down
	set_angle_pro(3,now_angle[2] + param, 1000);		//+ ->down
	delay(2000);

	}

void arm_capture() 	//爪子抓住
{
	set_angle_pro(5,-100,800);
	delay(2000);
}

void arm_open() 	//爪子放开
{
	set_angle_pro(5,300,1000);
	delay(1000);
}

void chassis(int angle_1) 		//底盘转动
{
	set_angle_pro(1,angle_1,1000);
	delay(1000);
	}

/****************************************
 * 以上操作均是对抓取高出物品的操作
 * 
*/

/***********************机械臂反曲的时候各舵机初始位置*************************/

int init_anti_arm_angle[5] = {800,-300,-250,-1280,200};

void anti_arm_init()
{
	//set_angle_pro(1,init_anti_arm_angle[0],1000);		//+ ->right
	set_angle_pro(2,init_anti_arm_angle[1],1000);   	//+ ->down
	set_angle_pro(3,init_anti_arm_angle[2],1000);		//+ ->down
	set_angle_pro(4,init_anti_arm_angle[3],1000);		//- -> up
	set_angle_pro(5,init_anti_arm_angle[4],1000);		//
	delay(3000);
}
//- -> arm_yan chang 负值 机械臂伸展
void anti_arm_around(int delta)
{
	set_angle_pro(3,now_angle[2] - delta, 1000);		//+ ->down
	set_angle_pro(4,now_angle[3] - delta, 1000);			//- -> up
	delay(2000);
}
//- -> arm_down    负值 机械臂下降
void anti_adjustment_up_dowm(int param)
{
	set_angle_pro(2,now_angle[1] + param, 1000);   		//+ ->down
	set_angle_pro(3,now_angle[2] + param, 1000);		//+ ->down
	delay(2000);
}	

//arm action group 1
//~ void catch_one()
//~ {
	//~ arm_ground();
	//~ delay(2000);
	//~ arm_ground_back();
	//~ delay(1500);
//~ } 
//arm action group 1  抓高出蔬菜
void catch_two(int catch_state)
{
	time_number = 0;
	pos_select = 2;
	adjust_flag_y = 1;
	adjust_flag_x = 0;
	
	forward_arm_init();  	// 机械臂正曲
	arm_around(800);
	switch(catch_state)
	{
		case 0:				//不使用摄像头修正
			arm_around(-50);
			//~ adjustment_up_dowm(350);
			arm_capture();
			adjustment_up_dowm(-350);
			pos_init_close();
			chassis(100);
			arm_open();
			break;
		case 1 :			//使用摄像头修正
			
			cam_start = 1;
			arm_adjust_flag=1;
			while(num_flag < 4 || num_flag_x < 4)
			{
				}	
			num_flag_x = 0;
			num_flag = 0;
			arm_adjust_flag=0;
			cam_start = 0;
			arm_capture();
			
			//~ set_angle_pro(2,now_angle[1] - 550, 1000);   		//+ ->down
			//~ set_angle_pro(3,now_angle[2] - 550, 1000);		//+ ->down
			
			//~ delay(1000);
			//arm_around(400);
			adjustment_up_dowm(-350);
			pos_init_close();
			chassis(100);
			arm_open();
			break;
		case 2:				//识别是否成熟,使用摄像头修正
			ripe_num = 0;
			cam_start = 1;
			delay(500);
			while(cam_start == 1)
			{
				if(ripe_num > 10)
				{
					
					cam_start = 0;
					pass_flag = 0;
					break;
					}
				delay(10);
			}
			if(pass_flag == 1)
			{
				printf("未成熟，跳过\n");
				laba(1);
				arm_around(-600);
				pos_init();
				
			}else{
				printf("成熟,开始抓取..............................................................\n");
				laba(2);
				cam_start = 1;
				arm_adjust_flag=1;
				while(num_flag<4)
				{
					delay(50);
					time_number++;
					if( time_number > 200)
					{
						break;
						}
					}	
				num_flag = 0;
				cam_start = 0;
				arm_adjust_flag=0;
				arm_capture();
				//~ set_angle_pro(2,now_angle[1] - 550, 1000);   		//+ ->down
				//~ set_angle_pro(3,now_angle[2] - 550, 1000);		//+ ->down
				//~ delay(1000);
				//arm_around(400);
				adjustment_up_dowm(-350);
				pos_init_close();
				chassis(100);
				arm_open();
			}
			break;
		
	}
	
}

// 抓地处南瓜
void catch_three(int catch_state)
{
	time_number = 0;
	pos_select = 1;
	adjust_flag_x = 1;
	adjust_flag_y = 0;
	//~ chassis(1100);	
	anti_arm_init(); 				// 机械臂反曲
	switch(catch_state)
	{
		case 0:
			adjustment_up_dowm(-150);
			arm_around(-1300);			//机械臂远端向下延申
			arm_capture();
			set_angle_pro(4,1000,800);
			delay(1000);
			
			set_angle_pro(3,-300,800);
			delay(1000);
			
			set_angle_pro(2,-100,800);
			delay(1000);
			
			pos_init_close();			//回到起始位置
			
			chassis(100);
			arm_open();
			break;
		case 1:
			
			set_angle(3,150,1000); 			//识别一下
			delay(2000);
			cam_start = 1;
			arm_adjust_flag=1;
			while(num_flag<4)
			{
				delay(50);
				time_number++;
				if( time_number > 200)
				{
					break;
					}
				}	
			num_flag = 0;
			cam_start = 0;
			arm_adjust_flag=0;
			
			adjustment_up_dowm(-150);
			arm_around(-1300);			//机械臂远端向下延申
			//~ adjustment_up_dowm(-250);
			//~ arm_around(150);			//机械臂远端向下延申
			arm_capture();
			set_angle_pro(4,1000,800);
			delay(1000);
		
			set_angle_pro(3,-300,800);
			delay(1000);
			
			set_angle_pro(2,-100,800);
			delay(1000);
			
			pos_init_close();			//回到起始位置
			
			chassis(100);
			arm_open();
			break;
		case 2:
			pic_null = 0;
			ripe_num = 0;
			set_angle(3,350,1000); 			//识别一下
			delay(2000);
			cam_start = 1;
			delay(500);
			while(cam_start == 1)
			{
				
				if(ripe_num > 30)
				{
					cam_start = 0;
					pass_flag = 0;
					break;
					}
			}
			if(pass_flag == 1)
			{
				if(pic_null == 0)
				{
					laba(1);
				}
				pass_flag = 0;
				//ripe_flag = 0;
				printf("未成熟，跳过\n");
				//laba(1);
				adjustment_up_dowm(450);
				pos_init_close();			//回到起始位置
				chassis(100);
			}else{
				printf("成熟,开始抓取\n");
				laba(2);
				//ripe_flag = 1;
				cam_start = 1;
				delay(500);
				arm_adjust_flag=1;
				while(num_flag<4)
				{
					delay(50);
					time_number++;
					if( time_number > 200)
					{
						break;
						}
					}	
				num_flag = 0;
				cam_start = 0;
				arm_adjust_flag=0;
				
				adjustment_up_dowm(-150);
				arm_around(-1300);			//机械臂远端向下延申
				
				adjustment_up_dowm(-250);
				arm_around(150);			//机械臂远端向下延申
				arm_capture();
				set_angle_pro(4,1000,800);
				delay(1000);
				
				if( now_angle[0] > 0 )
				{
				chassis(1000);
				}else if( now_angle[0] < 0)
				{
					chassis(-800);
					}
				
				set_angle_pro(3,-300,800);
				delay(1000);
				
				set_angle_pro(2,-100,800);
				delay(1000);
				
				pos_init_close();			//回到起始位置
				
				chassis(100);
				arm_open();
			}
			break;
		case 3:
			adjustment_up_dowm(-150);
			arm_around(-1300);			//机械臂远端向下延申
			
			adjustment_up_dowm(-250);
			arm_around(150);			//机械臂远端向下延申
			arm_capture();
			
				
			set_angle_pro(4,1000,800);
			delay(1000);
			
			set_angle_pro(3,-300,800);
			delay(1000);
			
			if( now_angle[0] > 0 )
			{
				chassis(1000);
				}else if( now_angle[0] < 0)
				{
					chassis(-800);
					}
					
			//~ set_angle_pro(2,-100,800);
			//~ delay(1000);
			
			pos_init_close();			//回到起始位置
			
			chassis(100);
			arm_open();
			break;
	}
	
}



void result_log(int res,int l_r)
{
	result[l_r] = res;
	}

void result_clesr()
{
	result[0] = -1;
	result[1] = -1;
	}


void Catch_C_part(int x,int l_or_r)
{
	if( action_start_2func == 1 )
	{
		if( result[0] == 1 )
		{
			chassis(-300);
			catch_three(3);
		}else{
			printf(",,,,,,,,,");
			}
		if( result[1] == 1 )
		{
			chassis(500);
			catch_three(3);
		}else{
			printf(",,,,,,,,,");
			}
		
		result_clesr();
		action_start_2func = 0;
		}
	delay(2000);
	if(0 == l_or_r)
	{
		chassis(1000);
		}
	else{
		chassis(-800);
		}
	set_angle_pro(2,-200,1000);   	//+ ->down
	set_angle_pro(3,-150,1000);		//+ ->down
	set_angle_pro(4,500,1000);		//- -> up
	
	delay(1000);
	set_angle_pro(2,-300,1000);   	//+ ->down
	set_angle_pro(3,-1050,1000);		//+ ->down
	set_angle_pro(4,300,1000);		//- -> up

	delay(1000);
	
	pic_null = 0;
	ripe_num = 0;
	cam_start = 1;
	delay(500);
	while(cam_start == 1)
	{
		if(ripe_num > 30)
		{
			
			cam_start = 0;
			pass_flag = 0;
			break;
			}
		delay(500);
	}
	switch(x)
	{
		case 0:				//Cpart_A
			if(pass_flag == 1)
			{
				if(pic_null == 0)
				{
					laba(1);
				}
				printf("未成熟，跳过\n");
				//laba(1);
				pos_init_close();			//回到起始位置
				chassis(100);
				arm_open();
			}else{
				printf("成熟,开始抓取\n");
				laba(2);
				set_angle_pro(2,-100,1000);   	//+ ->down
				set_angle_pro(3,-1150,1000);		//+ ->down
				set_angle_pro(4,200,1000);		//- -> up
				delay(1000);
				arm_capture();
				set_angle_pro(2,-100,1000);   	//+ ->down
				set_angle_pro(3,-1150,1000);		//+ ->down
				set_angle_pro(4,200,1000);		//- -> up
				delay(1000);
				pos_init_close();
				chassis(100);
				arm_open();
			}
			break;
		case 1:			//Cpart_B
			if(pass_flag == 1)
			{
				if(pic_null == 0)
				{
					laba(1);
				}
				printf("未成熟，跳过\n");
				//laba(1);
				res = 0;
				result_log(res,l_or_r);
			}else{
				printf("成熟,开始抓取\n");
				laba(2);
				res = 1;
				result_log(res,l_or_r);
			}
			pos_init_close();
			chassis(100);
			arm_open();
			break;
	}
}

void Dpart_arm_init()
{
	set_angle(10,700,1000);
	delay(1000);
	
	set_angle_pro(2,500,1000);   	//+ ->down
	set_angle_pro(3,1350,1000);		//+ ->down
	
	set_angle_pro(5,100,1000);		//
	delay(1000);
	set_angle_pro(4,1100,1000);		//- -> up
	delay(1000);
	set_angle_pro(2,-600,1000);   	//+ ->down
	set_angle_pro(3,850,1000);		//+ ->down
	set_angle_pro(4,800,1000);		//- -> up
	
	delay(1000);
	}
void D_detect(int id,int *point)
{
	D_x_one = 0;
	cam_start = 1;
	delay(1000);
	while(1)
	{
		

		if( 2 == D_x_one  )
		{
			point[id] = 0;
			printf("nothing...........");
			cam_start = 0;
			laba(2);	
			set_angle_pro(4,1000,800);
			delay(1000);
			
			set_angle_pro(3,-250,800);
			delay(1000);
			pos_init();
			break;
			}
		if( 1 == D_x_one )
		{
			point[id] = 1;
			printf("finish detect D part one ...............");
			cam_start = 0;
			laba(1);
			set_angle_pro(4,1000,800);
			delay(1000);
			
			set_angle_pro(3,-250,800);
			delay(1000);
			pos_init();
			break;
			}
		delay(10);
	}
}

void D_catch(int a,int drection)
{
	if(a == 1)
	{
		if( drection == 1)		// 1------- -x
		{
			set_angle(10,-700,1000);
			delay(1000);
			set_angle(10,700,2000);
			delay(2000);
		}else if( drection == 2 )		//2--------y
		{
			set_angle(11,-700,1000);
			delay(1000);
			set_angle(11,700,2000);
			delay(2000);
			}
	}

}

void arm_test()
{
	set_angle(1,1000,1000);		//+ ->right
	set_angle(2,500,1000);   	//+ ->down
	set_angle(3,850,1000);		//+ ->down
	set_angle(4,700,1000);		//- -> up
	set_angle(5,300,1000);		//
	delay(1000);
}


void adjust_car()
{
	set_angle_pro(2,-200,1000);   	//+ ->down
	set_angle_pro(3,-850,1000);		//+ ->down
	set_angle_pro(4,300,1000);		//- -> up
	delay(1000);
	}
/****************摄像头对接入系统****************
 * 蔬果在摄像头坐标中的目标位置
 * 对夹爪位置调整的标志位
 * 
 * */

//
//~ void adjust_arm()
void *adjust_arm(void *arg)//pid3
{
	//~ if(x-targ_pos_cam_x > 10 && adjust_flag_x == 1)
	//~ {
			//~ set(1, temp_angle1 - (x-targ_pos_cam_x)*3 , 1000)//逆时针
	//~ }
	//~ elseif(x-targ_pos_cam_x < -10 && adjust_flag_x == 1)
	//~ {
			//~ //顺时针
	//~ }else{
			//~ adjust_flag_x = 0;//停止
	//~ }
	while(1)
	{
		if(arm_adjust_flag == 1)
		{
			switch(pos_select)
			{
				case 1:
					if(x-targ_pos_cam_x > 20 && adjust_flag_x == 1)
					{
						chassis( now_angle[0] - 10);//底盘顺时针
						printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx\n");
					}
					else if (x-targ_pos_cam_x < -20 && adjust_flag_x == 1)
					{
						chassis( now_angle[0] + 10);//底盘逆时针
						/* code */
					}
					else
					{
						//~ if(adjust_flag_x == 1)
						num_flag++;
					}
				break;
				case 2:
					if(x-targ_pos_cam_x > 20)
					{
						chassis( now_angle[0] + 10);//底盘顺时针
						printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx\n");
					}
					else if (x-targ_pos_cam_x < -20)
					{
						chassis( now_angle[0] - 10);//底盘逆时针
						/* code */
					}
					else
					{
						//~ if(adjust_flag_x == 1)
						num_flag_x++;
					}
				break;
			}
			if(y-targ_pos_cam_y > 20 && adjust_flag_y == 1)
			{
				adjustment_up_dowm(y-targ_pos_cam_y);//向下
				printf("1111111111111111111111111111111111111111\n");
			}
			else if(y-targ_pos_cam_y < -20 && adjust_flag_y == 1)
			{
					adjustment_up_dowm(y-targ_pos_cam_y);//向上
			}else{
				if(adjust_flag_y == 1)
				{
					num_flag++;
				}
 			}
 			printf("num_flag ...........................: %d \n",num_flag);
		}
		delay(800);
		
	}
}

extern int num;
int main()
{
	
	wiringPiSetup();
	duoji_init();
	pos_init();
	
	pthread_create(&t4,NULL,ring_cpp,NULL);
	//pthread_create(&t1,NULL,adjust_arm,NULL);

	
	chassis(-800);
	Dpart_arm_init();
	D_detect(0,D_x);
	D_catch(D_x[0],1);
	
	while(1);	
	
	wiringPiSetup();
	duoji_init();
	erweima_init();
	motor_init();
	turn_wheel_y();
	char color_shen=0;
	signal(SIGINT, my_handler);
	signal(SIGABRT, my_handler_1);
	char *tx1="ACT0";
	fd_2=serialOpen("/dev/ttyAMA0",115200);
	serialPuts(fd_2,tx1);
	wiringPiSetup();
	if(wiringPiSetup()<0)
       return 1;
    pos_init();
    arm_open();
	motor_init();
	  
	//~ chassis(1350);
	//~ adjust_car();
	  
	OPS9_Init();							//初始化OPS-9
	serialPuts(fd_2,tx1);
	delay(6000);
	serialPuts(fd_2,tx1);
	
	pthread_create(&t1,NULL,fun1,NULL);		//开启线程
	//~ pthread_create(&t3,NULL,angle_a,NULL);
	
	pthread_create(&t4,NULL,ring_cpp,NULL);
	pthread_create(&t1,NULL,adjust_arm,NULL);
	pthread_create(&t6,NULL,fun6,NULL);		//开启线程
	//~ pthread_create(&t7,NULL,fun7,NULL);
	
	
	
	
	
	//~ car_calibrate_flag = 1;
	//~ chassis(-800);
	//~ set_angle_pro(2,-200,1000);   	//+ ->down
	//~ set_angle_pro(3,-150,1000);		//+ ->down
	//~ set_angle_pro(4,500,1000);		//- -> up
	
	//~ delay(1000);
	//~ set_angle_pro(2,-300,1000);   	//+ ->down
	//~ set_angle_pro(3,-1050,1000);		//+ ->down
	//~ set_angle_pro(4,300,1000);		//- -> up

	//~ delay(1000);
	//~ cam_adjust=1;
	//~ while(1);
	//~ fang_init();
	//~ delay(2000);
	//~ cam_adjust=1;
	//~ chassis(1300);
	//~ adjust_car();
	//~ while(1);
	//~ move_x_y(dir_y,-500,20,0);
	//~ move_x_y(dir_y,500,20,0);
	//~ delay(800);
	//~ move_x_y(dir_y,1500,20,0);
	//~ delay(1000);
	//~ move_x_y(dir_y,2500,20,0);
	//~ delay(1000);
	//~ move_x_y(dir_y,3200,20,0);     
	//~ delay(1000);
	//~ move_x_y(dir_x,950,20,0);		 //move_x_y(char dir,float target,int speed ,char lo),修改target可以更改车刚进入B的位置，target增大，车靠近C区，target减小，车靠近A区,其他值不用更改
	//~ delay(1000);
	//~ move_x_y(dir_y,2650,20,0);	
	//~ delay(1000);
	//~ move_x_y(dir_y,1680,20,0);
	//~ delay(1000);
	//~ move_x_y(dir_y,650,20,0);
	//~ delay(1000);
	
	 laba(3);//1不成熟,2成熟
	move_x_y(dir_y,480,20,0);
	delay(800);
	chassis(1100);
	catch_two(0);
	delay(800);
	chassis(1000);
	catch_three(0);   				//part----A-1
	
	delay(1000);
	move_x_y(dir_y,1500,20,0);
	delay(800);
	chassis(-800);
	catch_two(0);
	delay(800);
	chassis(-800);
	catch_three(0);   				//part----A-2
	
	delay(1000);
	move_x_y(dir_y,2500,20,0);
	delay(800);
	chassis(1100);
	catch_two(0);
	delay(800);
	chassis(1000);
	catch_three(0);   				//part----A-3
	
	delay(1000);
	move_x_y(dir_y,3200,20,0);     
	
	delay(1000);
	move_x_y(dir_x,930,20,0);		 //move_x_y(char dir,float target,int speed ,char lo),修改target可以更改车刚进入B的位置，target增大，车靠近C区，target减小，车靠近A区,其他值不用更改
	
	delay(1000);
	move_x_y(dir_y,2650,20,0);		//进入B区第1个位置//move_x_y(char dir,float target,int speed ,char lo),修改target可以更改车在B的位置，target增大，车靠近刚进入B区的位置，target减小，车靠近起始区,其他值不用更改
	delay(800);
	chassis(-800);
	catch_two(2);
	delay(800);
	chassis(-800);
	catch_three(2);   				//part----B-1
	delay(1000);
	
	move_x_y(dir_y,1670,20,0);		//进入B区第2个位置
	delay(800);
	chassis(1100);
	catch_two(2);
	delay(800);
	chassis(1000);
	catch_three(2);   				//part----B-2
	delay(1000);
	move_x_y(dir_y,650,20,0);		//进入B区第3个位置
	delay(800);
	chassis(-800);
	catch_two(2);
	delay(800);
	chassis(-800);
	catch_three(2);   				//part----B-3
	delay(1000);			
	move_x_y(dir_y,0,20,0);
	
	delay(1000);
	delay(1000);
	move_x_y(dir_x,2130,20,0);
	delay(1000);
	move_x_y(dir_y,600,20,0);

	Catch_C_part(0,0);

	Catch_C_part(0,1);
	delay(1000);
	
	move_x_y(dir_y,1050,20,0);

	Catch_C_part(0,0);

	Catch_C_part(0,1);
	delay(1000);
	
	move_x_y(dir_y,1450,20,0);
	Catch_C_part(0,0);

	Catch_C_part(0,1);
	delay(1000);
	
	move_x_y(dir_y,1850,20,0);
	Catch_C_part(0,0);

	Catch_C_part(0,1);
	delay(1000);
	
	move_x_y(dir_y,2250,20,0);
	Catch_C_part(0,0);

	Catch_C_part(0,1);
	delay(1000);
	//~ while(1);
	
	angle_corrective=angle;
	pid_flag=2;
	target_xx=0;
	serialPuts(fd_2,tx1);
	delay(6000);
	
	//~ delay(10000);
	move_x_y(dir_y,400,20,0);
	delay(1000);
	move_x_y(dir_x,900,20,0);
	delay(1000);
	move_x_y(dir_y,60,20,0);
	Catch_C_part(1,0);
	Catch_C_part(1,1);
	
	delay(1000);
	action_start_2func = 1;
	move_x_y(dir_y,-330,20,0);
	Catch_C_part(1,0);
	Catch_C_part(1,1);
	
	delay(1000);
	action_start_2func = 1;
	move_x_y(dir_y,-730,20,0);
	Catch_C_part(1,0);
	Catch_C_part(1,1);
	
	delay(1000);
	action_start_2func = 1;
	move_x_y(dir_y,-1130,20,0);
	Catch_C_part(1,0);
	Catch_C_part(1,1);
	
	delay(1000);
	action_start_2func = 1;
	move_x_y(dir_y,-1530,20,0);
	Catch_C_part(1,0);
	Catch_C_part(1,1);
	delay(1000);
	while(1);
	
	
	while(1);
	laba(1);
	target_xx=x_pos;
	move_x_y(dir_y,500,20,0);
	delay(800);
	chassis(1100);
	catch_two(1);
	delay(800);
	chassis(1000);
	catch_three(1);   //part----A-1
	
	move_x_y(dir_y,1500,20,0);
	delay(800);
	chassis(-800);
	catch_two(1);
	delay(800);
	chassis(-800);
	catch_three(1);   //part----A-2
	
	
	move_x_y(dir_y,2500,20,0);
	delay(800);
	chassis(1100);
	catch_two(1);
	delay(800);
	chassis(1000);
	catch_three(1);   //part----A-3
	
	delay(2000);
	move_x_y(dir_y,3000,20,0);
	
	move_x_y(dir_x,1000,20,0);
	move_x_y(dir_y,2500,20,0);
	
	return 0;
}
